function [ pd ] = distort( p, K, D )
%DISTORT Distort a homogenous pixel co-ordinate.
%   Given a homogenous pixel with co-ordinates p = [x y w] (or [u v 1]),
% an intrinsics matrix K = [fx gamma cx; 0 fy cy; 0 0 1] and distortion 
% coefficients D = [k1 k2 p1 p2 k3], returns the distorted pixel 
% co-ordinates pd = [u' v' 1].

% Copyright (c) Michael Warren 2014

% This file is part of the AMMCC Toolbox.

% The AMMCC Toolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% The AMMCC Toolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Lesser General Public License
% along with the AMMCC Toolbox.  If not, see <http://www.gnu.org/licenses/>.

    while(size(D,2) < 8)
        D = [D 0];
    end

    pp = inv(K)*p;
    pp = pp./pp(3);

    x = pp(1);
    y = pp(2);
    r2 = x*x+y*y;
    r4 = r2*r2;
    r6 = r4*r2;
    xr = x*(1+D(1)*r2+D(2)*r4+D(5)*r6);
    yr = y*(1+D(1)*r2+D(2)*r4+D(5)*r6);

    xp = 2*D(3)*x*y+D(4)*(r2+2*x*x);
    yp = 2*D(4)*x*y+D(3)*(r2+2*y*y);
    xd = xr+xp;
    yd = yr+yp;

    pd = K*[xd; yd; 1];

    pd = pd./pd(3);
end

